#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/twist.hpp>

class LidarObstacleAvoider : public rclcpp::Node {
public:
    LidarObstacleAvoider() : Node("lidar_obstacle_avoider") {
        // 声明参数
        this->declare_parameter("safety_distance", 1.5f);
        this->declare_parameter("forward_speed", 0.2f);
        this->declare_parameter("turn_speed", 0.5f);
        this->declare_parameter("front_angle", 30.0f); // 前方检测角度（度）
        
        // 获取参数值
        this->get_parameter("safety_distance", safety_distance_);
        this->get_parameter("forward_speed", forward_speed_);
        this->get_parameter("turn_speed", turn_speed_);
        this->get_parameter("front_angle", front_angle_);
        
        // 转换角度为弧度
        front_angle_rad_ = front_angle_ * M_PI / 180.0f;
        
        subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan", 10, std::bind(&LidarObstacleAvoider::lidar_callback, this, std::placeholders::_1));
        
        cmd_vel_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
    }

private:
    void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
        auto command = geometry_msgs::msg::Twist();
        bool obstacle_detected = false;
        
        // 计算前方检测区域的索引范围
        int num_ranges = msg->ranges.size();
        float angle_increment = msg->angle_increment;
        int front_indices = static_cast<int>(front_angle_rad_ / angle_increment);
        int mid = num_ranges / 2;
        int start_idx = mid - front_indices;
        int end_idx = mid + front_indices;
        
        // 确保索引在有效范围内
        start_idx = std::max(0, start_idx);
        end_idx = std::min(num_ranges - 1, end_idx);
        
        // 检查前方区域是否有障碍物
        for (int i = start_idx; i <= end_idx; ++i) {
            if (!std::isnan(msg->ranges[i]) && msg->ranges[i] < safety_distance_) {
                obstacle_detected = true;
                break;
            }
        }
        
        if (obstacle_detected) {
            // 检测到障碍物，决定转向方向
            // 计算左右两侧的平均距离
            float left_avg = calculate_average_distance(msg, 0, mid - front_indices);
            float right_avg = calculate_average_distance(msg, mid + front_indices, num_ranges - 1);
            
            // 选择更安全的方向（距离更远的一侧）
            if (left_avg > right_avg) {
                command.angular.z = turn_speed_;  // 向左转
                RCLCPP_INFO(this->get_logger(), "Obstacle detected, turning left.");
            } else {
                command.angular.z = -turn_speed_;  // 向右转
                RCLCPP_INFO(this->get_logger(), "Obstacle detected, turning right.");
            }
            
            // 转向时停止前进
            command.linear.x = 0.0;
        } else {
            // 没有检测到障碍物，向前移动
            command.linear.x = forward_speed_;
            command.angular.z = 0.0;
        }
        
        cmd_vel_publisher_->publish(command);
    }
    
    // 计算指定范围内的平均距离
    float calculate_average_distance(const sensor_msgs::msg::LaserScan::SharedPtr msg, int start, int end) {
        float sum = 0.0f;
        int count = 0;
        
        for (int i = start; i <= end; ++i) {
            if (!std::isnan(msg->ranges[i]) && !std::isinf(msg->ranges[i])) {
                sum += msg->ranges[i];
                count++;
            }
        }
        
        return count > 0 ? sum / count : 0.0f;
    }

    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
    
    // 参数
    float safety_distance_;    // 安全距离
    float forward_speed_;      // 前进速度
    float turn_speed_;         // 转向速度
    float front_angle_;        // 前方检测角度（度）
    float front_angle_rad_;    // 前方检测角度（弧度）
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<LidarObstacleAvoider>());
    rclcpp::shutdown();
    return 0;
}
